#pragma once

#include <rclcpp/rclcpp.hpp>
#include <cmath>

#include <geometry_msgs/msg/twist_stamped.hpp>
#include <sensor_msgs/msg/joy.hpp>
#include <sensor_msgs/msg/joy_feedback_array.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <std_msgs/msg/color_rgba.hpp>
// #include <std_srvs/srv/trigger.hpp>
#include "agv_msgs/msg/pair_type_int.hpp"
#include "agv_srvs/srv/outputs_new.hpp"

#include "agv_srvs/srv/set_operation_mode.hpp"

#include <memory>
#include "agv_msgs/msg/periph_teleop.hpp"
#include "agv_msgs/msg/agv_mode.hpp"
#include "agv_msgs/msg/hd1000.hpp"
// #include "agv_msgs/msg/event.hpp"
// #include "agv_msgs/msg/event_occur.hpp"
#include "agv_msgs/msg/agv_event_status.hpp"
#include <std_msgs/msg/float64.hpp>
#include <thread>
#include "agv_msgs/msg/read_in_puts.hpp"
#include "agv_msgs/msg/read_in_puts_new.hpp"
#include "agv_msgs/msg/steer_dir_feed_back.hpp"
#include "agv_msgs/msg/odometry_lite.hpp"
#include "std_msgs/msg/u_int8.hpp"
#include "std_msgs/msg/u_int8_multi_array.hpp"
#include "std_msgs/msg/u_int16_multi_array.hpp"
#include "std_msgs/msg/multi_array_dimension.hpp"

#include "toolKit.h"

#define SET_MODE_MANUL "manual"
#define SET_MODE_HALF_AUTO "halfAuto"
#define SET_MODE_AUTO "auto"
#define SET_MODE_IDLE "idle"

#define TIMEOUT_S (2.0)

/* 定义电机速度曲线类型枚举 */
typedef enum SpeedCurve {
  // CURVE_NONE=0,  //直启
  CURVE_TRAP= 0,  //梯形曲线
  CURVE_SPTA= 1  //S型曲线
}SpeedCurveType;

/* 定义按钮速度方向类型枚举 */
typedef enum VelocityFactor {
  Factor_NONE= 0, //NONE
  Factor_VX= 1,   //VX
  Factor_VY= 2,   //VY
  Factor_VZ= 3    //VA
}VelocityFactorType;

class ManHandler : public rclcpp::Node
{
public:
  ManHandler();
  ~ManHandler();

  //
  // Let "from" approaches "to" at the increament of "step".
  //
  // Note:
  //    1. "from" and "to" can be any value, but "fStep" should be positive.
  //    2. If "from" reaches "to", TRUE is returned.
  //
  template <class Type>
  bool Approach(Type &from, Type to, Type step)
  {
      if (from + step < to)
      {
          from += step;
          return false;
      }

      else if (from - step > to)
      {
          from -= step;
          return false;
      }

      else
      {
          from = to;
          return true;
      }
  }

  template <class Type>
  void readParam(std::string prefix, Type &value, Type default_val)
  {
    this->get_parameter_or(prefix, value, default_val);
    RCLCPP_INFO(this->get_logger(), "%s: %f", prefix.c_str(), static_cast<float>(value));
  }

  /* 定义电机速度曲线对象 */
  typedef struct CurveObject {
    float startSpeed;    //开始调速时的初始速度
    float currentSpeed;   //当前速度
    float targetSpeed;   //目标速度
    float targetAngle;   //目标角度，°
    float stepSpeed;    //加速度
    float accRT;        //加速度realTime
    float jerk;        //加加速度(Jerk)],加速度本身变化的快慢
    float maxSpeed;     //最大速度
    float minSpeed;     //最小速度
    uint32_t aTimes;    //调速时间
    uint32_t maxTimes;   //调速跨度
    SpeedCurveType curveMode;  //曲线类型
    float flexible;     //S曲线拉伸度
    float angle_ramp;
  }CurveObjectType;

  struct periph_read_input
  {
    bool chassisMode; 
    bool forwardMove;
    bool backwardMove;
    bool leftCrab;
    bool rightCrab;
    bool leftTurn;
    bool rightTurn;
    bool lift;
    bool fall;
    bool deepPressed;
    
    periph_read_input():chassisMode(false),
                        forwardMove(false),
                        backwardMove(false),
                        leftCrab(false),
                        rightCrab(false),
                        leftTurn(false),
                        rightTurn(false),
                        lift(false),
                        fall(false),
                        deepPressed(false){}
    void reset_signal(){
      forwardMove= false;
      backwardMove= false;
      leftCrab= false;
      rightCrab= false;
      leftTurn= false;
      rightTurn= false;
      deepPressed= false;
    }                
  };
  typedef struct periph_read_input read_input_data;

  struct valid_s
  {
    bool valid;
    std::chrono::system_clock::time_point new_come_time;
    valid_s():valid(false){
      new_come_time= std::chrono::system_clock::now();
    }
    void update(void){
          valid= true;
      new_come_time = std::chrono::system_clock::now();
      }

    bool timeoutCheck(void){
      bool ret= false;
      double elapsed_secs = 0.0;  
      elapsed_secs = std::chrono::duration_cast<std::chrono::duration<double>>((std::chrono::system_clock::now() - new_come_time)).count();
      if (elapsed_secs > TIMEOUT_S){
        valid= false;
        ret= true;
      }
      return ret;
    }
  };
  typedef struct valid_s valid_t;

private:
  CurveObjectType curve_;
  float vmax_angular_;
  double acc_angular_;
  VelocityFactorType velFactor_;

  //IO按钮盒手动速度
  double  ioVelocityLinear_,
          ioHighVelocityLinear_,
          ioVelocityAngular_,
          ioHighVelocityAngular_;      

  std::mutex mutex_;

  std::thread thread_;
  std::timed_mutex quit_mutex_;

  agv_msgs::msg::AGVMode mode_;
  agv_msgs::msg::SteerDirFeedBack steer_;
  agv_msgs::msg::OdometryLite odom_;
  geometry_msgs::msg::TwistStamped  zero_vel_;
  agv_msgs::msg::AGVEventStatus event_;
  enum periphTeleopBut
  {
    NO_ACTIVE = 0,
    UP,
    DOWN,
    CLOCKWISE_ROTATION,
    ANTICLOCKWISE_ROTAION
  };
  

  rclcpp::Subscription<agv_msgs::msg::AGVMode>::SharedPtr agvmode_sub_;
  rclcpp::Subscription<std_msgs::msg::UInt16MultiArray>::SharedPtr joy_sub_;
  rclcpp::Subscription<agv_msgs::msg::AGVEventStatus>::SharedPtr eventOccurSub_;
  rclcpp::Subscription<agv_msgs::msg::ReadInPutsNew>::SharedPtr handlerEnableSub_;
  rclcpp::Subscription<agv_msgs::msg::SteerDirFeedBack>::SharedPtr steerSub_;
  rclcpp::Subscription<agv_msgs::msg::OdometryLite>::SharedPtr odomSub_;
  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr daemonSub_;
  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr keyboardSub_;
  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr keyboardOverrideSub_;
  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr joyOverrideSub_;

  rclcpp::Publisher<agv_msgs::msg::PeriphTeleop>::SharedPtr periphTeleop_pub_;
  rclcpp::Publisher<agv_msgs::msg::HD1000>::SharedPtr manHandler_pub_;
  rclcpp::Publisher<std_msgs::msg::UInt8MultiArray>::SharedPtr handlerLED_pub_;
  agv_msgs::msg::HD1000  hand_state_;
  agv_msgs::msg::HD1000  null_state_;

  // add fork lift action 2023/6/13
  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr man_override_pub_;
  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr man_vel_pub_;
  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr semi_override_pub_;
  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr semi_vel_pub_;
  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr fork_lift_pub_;
  std_msgs::msg::Float64 fork_lift_;
  std_msgs::msg::Float64 fork_lift_old_;

  //add 
  rclcpp::Client<agv_srvs::srv::SetOperationMode>::SharedPtr operationModeClient_;
  rclcpp::Client<agv_srvs::srv::OutputsNew>::SharedPtr outputMapClient_;
  
  read_input_data inputs_;   
  CDataTimeStamp<bool>  hd1000_active_;
  CDataTimeStamp<bool>  joy_active_;
  CDataTimeStamp<geometry_msgs::msg::TwistStamped>  button_vel_;
  CDataTimeStamp<geometry_msgs::msg::TwistStamped>  keyboard_vel_;
  CDataTimeStamp<geometry_msgs::msg::TwistStamped>  keyboardOverride_vel_;
  CDataTimeStamp<geometry_msgs::msg::TwistStamped>  daemon_vel_;

public:
  void handlerDevice(void);
  void keyboardVel(void);
  void speedLimitKeep(geometry_msgs::msg::TwistStamped &vel);
  void init();
  void buttonVel(bool have);
  static void thread_fun(ManHandler *pThis);
  void setLight(int mode);
  void joy_cb(const std_msgs::msg::UInt16MultiArray::SharedPtr multArr);
  void event_Callback(const agv_msgs::msg::AGVEventStatus::SharedPtr event);
  void agvmodeCallback(const agv_msgs::msg::AGVMode::SharedPtr state_p);
  void keyboardCallback(const geometry_msgs::msg::TwistStamped::SharedPtr pMsg);
  void keyboardOverrideCallback(const geometry_msgs::msg::TwistStamped::SharedPtr pMsg);
  void steerCallback(const agv_msgs::msg::SteerDirFeedBack::SharedPtr state_p);
  void odomCallback(const agv_msgs::msg::OdometryLite::SharedPtr odom_p);
  void daemonCallback(const geometry_msgs::msg::TwistStamped::SharedPtr daemon_p);
  void inputmap_Callback(const agv_msgs::msg::ReadInPutsNew::SharedPtr msg);
  bool getBit(int n, int k)
  {
      bool bx;
      if(((n >> k) & 1) == 1)
          bx = true;
      else
          bx = false;
      return bx;
  };
  bool haveVel(geometry_msgs::msg::TwistStamped vel)
  {
    return ( vel.twist.linear.x != 0.0 || vel.twist.linear.y != 0.0 || vel.twist.angular.z != 0.0);
  };
  bool haveVel(double vx, double vy, double va)
  {
    return (vx != 0.0 || vy != 0.0 || va != 0.0);
  };
  void resultantVelocityCurve(CurveObjectType *curve);
  void CalCurveSPTA(CurveObjectType *spta);
  void CalCurveTRAP(CurveObjectType *trap);
  void Velocitydecomposition(float resultant_vel, float angle);
  void periphPub(void);
  // 针对某些车型触边会断驱动器电源手柄增加屏蔽信号
  void bumperShiled();
  void callOutputSrv(std::string outStr, bool needOpen);
  int driveControl_;
  int bumperShield_;
  std::chrono::system_clock::time_point new_come_time_keyboard_;

  void changeOpMode_Req(uint8_t modetype);
  VelocityFactorType factorSelect(geometry_msgs::msg::TwistStamped twist);
  bool reverseCheck(geometry_msgs::msg::TwistStamped newVel, geometry_msgs::msg::TwistStamped lastVel);
  bool reverseCheck(double max_new, double max_last);
  double get_max(geometry_msgs::msg::TwistStamped vel);
  bool init_parameters();
};